/******************************************************************************
 *  _____     _____
 *  \  __\    \  __\
 *   \ \_\\    \ \_\\
 *    \  __\SS  \  __\roject
 *     \_\       \_\
 *
 * Mateusz Maciąg, Rafał Urbański
 * Komputerowe Systemy Sterowania
 *****************************************************************************/

#include "SimulatorEngine.h"

#include "SISOModel.h"

#include "DiscreteObject.h"

#include "SquareWaveform.h"
#include "SineWaveform.h"
#include "TriangleWaveform.h"
#include "StepWaveform.h"
#include "KroneckerWaveform.h"
#include "RandomWaveform.h"
#include "SweepWaveform.h"
#include "KubaWaveform.h"

#include "GainController.h"
#include "PIDController.h"
#include "GPCController.h"
#include "OpenLoopController.h"
#include "WrmnkTestController.h"
#include "RSTController.h"

#include "Jano/NLSaturation.h"

#include "SimFile/ConfigWriter.h"
#include "SimFile/ConfigReader.h"
#include "StringConstants.h"

#include "Utils.h"

#include <cmath>
#include <vector>
#include <fstream>

#include <fstream>

namespace RafMat
{
namespace SimEngine
{

using namespace std;
using namespace boost;
using namespace SimInterface;
using namespace SimFile;

SimulatorEngine::SimulatorEngine()
{
    m_strToCtrl["P"] = ControllerTypeGain;
    m_strToCtrl["PID"] = ControllerTypePID;
    m_strToCtrl["GPC"] = ControllerTypeGPC;
    m_strToCtrl["WT"] = ControllerTypeWRMNKTest;
    m_strToCtrl["RST"] = ControllerTypeRST;
    m_strToCtrl["default"] = ControllerTypeOpenLoop;

    m_strToInput["kronecker"] = InputTypeKronecker;
    m_strToInput["step"] = InputTypeStep;
    m_strToInput["sine"] = InputTypeSine;
    m_strToInput["square"] = InputTypeSquare;
    m_strToInput["triangle"] = InputTypeTriangle;
    m_strToInput["random"] = InputTypeRandomNoise;
    m_strToInput["sweep"] = InputTypeSweep;
    m_strToInput["kuba"] = InputTypeKuba;

    m_strToNL["NLSaturation"] = NonlinearityNLSaturation;

    createController(ControllerTypeOpenLoop, false);
    setNonlinearity(NonlinearityNone);
    reset();

    // Set default exetucable path
    setCaptureFilePath("output.txt");
}


SimulatorEngine::ControllerMap SimulatorEngine::getControllerMap() const
{
    return m_strToCtrl;
}


SimulatorEngine::InputMap SimulatorEngine::getInputMap() const
{
    return m_strToInput;
}

IDiscreteObjectSPtr SimulatorEngine::createObject(int aDegree, int bDegree, int cDegree, int agDegree, int d, int delay)
{
    DiscreteObjectSPtr freshObject(new DiscreteObject(*this, aDegree, bDegree, cDegree, agDegree, d, delay));
    m_objectChain.push_back(freshObject);
    return freshObject;
}

void SimulatorEngine::removeObject(IDiscreteObjectSPtr obj)
{
    m_objectChain.remove(boost::dynamic_pointer_cast<DiscreteObject>(obj));
}

SimInterface::IConfigurableSPtr SimulatorEngine::setNonlinearity(NonlinearityType nlType)
{
    switch(nlType)
    {
    case NonlinearityNone:
        m_nonlinearityEnabled = false;
        m_outputNL.reset();
        break;

    case NonlinearityNLSaturation:
        m_nonlinearityEnabled = true;
        m_outputNL.reset(new NLSaturation(*this));
        break;
    }

    return static_pointer_cast<IConfigurable>(m_outputNL);
}

SimInterface::IConfigurableSPtr SimulatorEngine::getNonlinearityObj() const
{
    return static_pointer_cast<IConfigurable>(m_outputNL);
}

IConfigurableSPtr SimulatorEngine::getController()
{
    return static_pointer_cast<IConfigurable>(m_controller);
}

IConfigurableSPtr SimulatorEngine::createController(ControllerType ctrlType, bool copyInputs)
{
    WaveformContainerSPtr copy;

    // Utworzenie nowego regulatora wiąże się z utratą konfiguracji
    // wartości zadanych. Dlatego użytkownik może wybrać, czy wykonać
    // kopię poprzedniego stanu wejścia układu regulacji.
    if(copyInputs)
        copy = m_controller->getWaveformContainer();
    else
        copy.reset(new WaveformContainer(*this));

    // Dynamiczne utworzenie obiektów regulatorów.
    switch(ctrlType)
    {
    case ControllerTypeGain:
        m_controller.reset(new GainController(*this));
        break;
    case ControllerTypePID:
        m_controller.reset(new PIDController(*this));
        break;
    case ControllerTypeGPC:
        m_controller.reset(new GPCController(*this));
        break;
    case ControllerTypeOpenLoop:
        m_controller.reset(new OpenLoopController(*this));
        break;
    case ControllerTypeWRMNKTest:
        m_controller.reset(new WrmnkTestController(*this));
        break;
    case ControllerTypeRST:
        m_controller.reset(new RSTController(*this));
        break;
    default:
        throw logic_error("Unsupported type of controller.");
    }

    m_controller->setWaveformContainer(copy);

    return static_pointer_cast<IConfigurable>(m_controller);
}

SimInterface::IConfigurableSPtr SimulatorEngine::addInput(InputType inputType)
{
    WaveformContainerSPtr wcontainer = m_controller->getWaveformContainer();
    WaveformBaseSPtr inputPtr;

    switch(inputType)
    {
    case InputTypeSquare:
        inputPtr.reset(new SquareWaveform(*this));
        break;
    case InputTypeSine:
        inputPtr.reset(new SineWaveform(*this));
        break;
    case InputTypeTriangle:
        inputPtr.reset(new TriangleWaveform(*this));
        break;
    case InputTypeStep:
        inputPtr.reset(new StepWaveform(*this));
        break;
    case InputTypeKronecker:
        inputPtr.reset(new KroneckerWaveform(*this));
        break;
    case InputTypeRandomNoise:
        inputPtr.reset(new RandomWaveform(*this));
        break;
    case InputTypeKuba:
        inputPtr.reset(new KubaWaveform(*this));
        break;
    case InputTypeSweep:
        inputPtr.reset(new SweepWaveform(*this));
        break;
    default:
        throw logic_error("Unsupported type of input.");
    }

    wcontainer->insertWaveform(inputPtr);

    return static_pointer_cast<IConfigurable>(inputPtr);
}

void SimulatorEngine::removeInput(SimInterface::IConfigurableSPtr inputConf)
{
    WaveformBaseSPtr inputBase = dynamic_pointer_cast<WaveformBase>(inputConf);
    m_controller->getWaveformContainer()->removeWaveform(inputBase);
}

double SimulatorEngine::applyNonlinearity(double input)
{
    if(m_nonlinearityEnabled)
        return m_outputNL->simulateStep(input);
    else return input;
}

IConfigurableSPtr SimulatorEngine::getExternalPlant() const
{
    return static_pointer_cast<IConfigurable>(m_externalPlant);
}

IConfigurableSPtr SimulatorEngine::enableExternalPlant(bool enable)
{
    m_externalModeEnabled = enable;

    if(enable && !m_externalPlant)
    {
        m_externalPlant.reset(new ExternalPlant(*this));
    }

    if(!enable)
    {
        // Unload external plant.
        m_externalPlant.reset();
    }

    return getExternalPlant();
}

void SimulatorEngine::doStep()
{  


    m_currentControlTrick = m_controller->currentOutput();

    try {
        // Realizacja sprzężenia zwrotnego
        // m_currentOutput = y(i)
        m_controller->simulateStep(m_currentOutput);
    }
    catch(...)
    {
    }

    m_currentInput = m_controller->currentSetPoint();
    m_currentOutputTrick = m_currentOutput;

    if(m_externalModeEnabled)
    {
        m_currentOutput = m_externalPlant->simulateStep(m_controller->currentOutput());
        m_sample++;
        captureData();
        return;
    }

    // Jeśli nie ma żadnych obiektów, to przekaż
    // wyjście regulatora bezpośrednio na wyjscie układu regulacji.
    if(m_objectChain.size() < 1)
    {
        m_currentOutput = applyNonlinearity(m_controller->currentOutput());
    }
    else
    {
        // Propagacja wyjścia regulatora przez łańcuch obiektów
        std::list<DiscreteObjectSPtr>::iterator it;

        // Przekaż wejście pierwszemu obiektowi w łańcuchu:
        it = m_objectChain.begin();
        double tmpOut = (*it)->simulateStep(m_controller->currentOutput());

        for(++it; it != m_objectChain.end(); ++it)
        {
            // Propagacja sygnału przez kolejne obiekty;
            tmpOut = (*it)->simulateStep(tmpOut);
        }

        // Ustaw aktualne wyjście układu regulacji
        m_currentOutput = Utils::limitValue(applyNonlinearity(tmpOut), -1e6, 1e6);
    }

    m_sample++; // próbka i+1

    captureData();
}

void SimulatorEngine::captureData()
{
    if(!m_captureStream || !m_captureStream->is_open())
        return;

    if(m_captureEnabled)
    {
        *m_captureStream << m_sample << ";"
                    << m_currentInput << ";"
                    << m_controller->currentOutput() << ";"
                    << m_currentOutputTrick << ";"
                    << m_controller->currentError() << ";"
                    << endl;
    }
}

void SimulatorEngine::reset()
{
    std::list<DiscreteObjectSPtr>::iterator it;
    for(it = m_objectChain.begin(); it != m_objectChain.end(); ++it)
    {
        (*it)->reset();
    }

    m_controller->reset();

    m_sample = 0;
    m_currentInput = 0;
    m_currentOutputTrick = m_currentOutput = 0;
    m_currentControlTrick = 0;

    if(m_nonlinearityEnabled)
        m_outputNL->reset();

    if(m_externalPlant)
        m_externalPlant->reset();
}

vector<IDiscreteObjectSPtr> SimulatorEngine::getObjects() const
{
    return vector<IDiscreteObjectSPtr>(m_objectChain.begin(), m_objectChain.end());
}

std::vector<IConfigurableSPtr> SimulatorEngine::getInputs() const
{
    const vector<WaveformBaseSPtr> &wfrm = this->m_controller->getWaveforms();

    return vector<IConfigurableSPtr>(wfrm.begin(), wfrm.end());
}

void SimulatorEngine::saveConfiguration(std::string filePath)
{
    ofstream outFile(filePath.c_str());

    if(outFile.fail())
    {
        throw std::logic_error(string("Cannot open file ") + filePath);
    }

    ConfigWriter cfg(outFile);

    cfg.pushMultilineComment(StringConstants::configInfoPrologue);
    cfg.pushEmptyLine(2);
    cfg.pushMultilineComment(StringConstants::configInfoDescription);
    cfg.pushMultilineComment(StringConstants::configInfoSimulationSpec);
    cfg.pushBlock(ConfigWriter::BlockSimulation, "Symulator");
    cfg.pushLineComment("Blok konfiguracujny symulatora");

    // Regulator
    cfg.pushEmptyLine();
    cfg.pushMultilineComment(StringConstants::configInfoModelSpec);
    list<DiscreteObjectSPtr>::iterator it;
    for(it = m_objectChain.begin(); it != m_objectChain.end(); ++it)
    {
        cfg << *it->get();
    }

    cfg.pushEmptyLine();
    cfg.pushMultilineComment(StringConstants::configInfoControllerSpec);
    cfg << *m_controller.get();

    // Wejścia
    WaveformContainer &wcont = *m_controller->getWaveformContainer().get();

    cfg.pushEmptyLine();
    cfg.pushMultilineComment(StringConstants::configInfoInputSpec);
    cfg << wcont;

    // Nieliniowość

    if(m_nonlinearityEnabled)
    {
        cfg.pushEmptyLine();
        cfg.pushMultilineComment("Nieliniowości układu");
        cfg << *m_outputNL.get();
    }

}

// Na potrzeby funkcji loadConf*
typedef vector<BlockInfoSPtr>::const_iterator BlkIterator;

void SimulatorEngine::loadConfiguration(std::string filePath)
{
    ifstream inFile(filePath.c_str());

    if(inFile.fail())
    {
        throw std::logic_error(string("Cannot open file ") + filePath);
    }

    ConfigReader cfg(inFile);
    BlockInfoSPtr root = cfg.getConfiguration();

    if(root->id() != "Simulation")
        throw conf_reader_error::semanticError(*root, "Root node name should be 'Simulation'.");

    // Pobierz wszystkie podbloki bloku Simulation.
    vector<BlockInfoSPtr> confBlocks = root->children();

    // Usuń ewentualnie istniejące obiekty w symulatorze.
    clearAll();

    unsigned ctrlInstances = 0; // Zliczanie regulatorów w pliku, ma być max. 1
    unsigned nlInstances = 0; // Zliczanie nieliniowości

    for(BlkIterator it = confBlocks.begin(); it != confBlocks.end(); ++it)
    {
        if((*it)->id() == "Model")
        {
            loadConfModel(*it);
        }
        else if((*it)->id() == "Controller")
        {
            if(++ctrlInstances > 1)
                throw conf_reader_error::semanticError(**it, "Only one 'Controller' block can be supported");
            loadConfController(*it);
        }
        else if((*it)->id() == "Input")
        {
            loadConfInput(*it);
        }
        else if((*it)->id() == "Nonlinearity")
        {
            if(++nlInstances > 1)
                throw conf_reader_error::semanticError(**it, "Only one 'Nonlinearity' block can be supported");
            loadConfNonlinearity(*it);
        }
        else throw conf_reader_error::semanticError(**it,
                                                    "Unknown block type. Only 'Model', 'Controller', 'Input', 'Nonlinearity' are suppoted.");
    }

    if(nlInstances == 0) // domyślne
        setNonlinearity(NonlinearityNone);
}

void SimulatorEngine::loadConfModel(BlockInfoSPtr modelBlock)
{
    DiscreteObjectSPtr dObj(new DiscreteObject(*this));
    loadConfPumpParams(modelBlock, dObj);

    for(BlkIterator it = modelBlock->children().begin(); it != modelBlock->children().end(); ++it)
    {
        BlockInfoSPtr params = *it;
        if(params->id() != "Params")
            throw conf_reader_error::semanticError(*params, "'Model' block can contain only 'Params' block.");

        DiscreteObjectParamsSPtr dParams(new DiscreteObjectParams(*dObj));
        BlockInfo::AssignmentMap assgn = (*it)->assignments();
        // Niestety beznadziejnie napisane i ważna jest kolejność
        // bpoly przed apoly - inaczej źle się obliczy szum.
        dParams->setParameter("cpoly", assgn["cpoly"]);
        dParams->setParameter("bpoly", assgn["bpoly"]);
        dParams->setParameter("apoly", assgn["apoly"]);
        dParams->setParameter("delay", assgn["delay"]);
        dParams->setParameter("agpoly", assgn["agpoly"]);
        dParams->setParameter("d", assgn["d"]);
        dParams->setParameter("switch_time", assgn["switch_time"]);
        dParams->setName((*it)->name());
        dObj->insertParameters(dParams);
    }

    dObj->updateParameters();
    iDiscreteObjectToConfigurable(dObj)->setName(modelBlock->name());
    m_objectChain.push_back(dObj);
}

void SimulatorEngine::loadConfController(BlockInfoSPtr ctrlBlock)
{
    string strType = (*ctrlBlock)["type"];
    ControllerType ctrlType = m_strToCtrl[strType];

    // Tworzymy kontroler. Ważne by dać opcję kopiowania wejść, gdyż
    // w pliku konf. wejścia mogą się pojawić przed blokiem regulatora.
    IConfigurableSPtr ctrl = createController(ctrlType, true);
    loadConfPumpParams(ctrlBlock, ctrl);
}

void SimulatorEngine::loadConfInput(BlockInfoSPtr inputBlock)
{
    InputType inputType = m_strToInput[(*inputBlock)["type"]];
    IConfigurableSPtr input = addInput(inputType);
    loadConfPumpParams(inputBlock, input);
}

void SimulatorEngine::loadConfPumpParams(BlockInfoSPtr blk, IConfigurableSPtr target)
{
    BlockInfo::AssignmentMap assgn = blk->assignments();

    // Zacznijmy od ustawienia nazwy obiektu:
    target->setName(blk->name());

    for(BlockInfo::AssignmentMap::reverse_iterator it = assgn.rbegin(); it != assgn.rend(); ++it)
    {
        // Nie uwzględniamy specjalnego parametru type:
        if(it->first == "type") continue;
        target->setParameter(it->first, it->second);
    }
}

void SimulatorEngine::loadConfNonlinearity(SimFile::BlockInfoSPtr nlBlock)
{
    NonlinearityType nlType = m_strToNL[(*nlBlock)["type"]];
    IConfigurableSPtr nlObj = setNonlinearity(nlType);

    if(nlObj)
        loadConfPumpParams(nlBlock, nlObj);
}

void SimulatorEngine::clearAll()
{
    m_objectChain.clear();
    this->createController(ControllerTypeOpenLoop, false);
    this->reset();
}

void SimulatorEngine::enableDataCapture(bool enable)
{
    m_captureEnabled = enable;

    if(m_captureEnabled)
    {
        m_captureStream.reset(new ofstream(m_capturePath.c_str()));
    }
    else
    {
        m_captureStream.reset();
    }
}

void SimulatorEngine::setCaptureFilePath(const std::string& path)
{
    m_capturePath = path;

    if(m_captureEnabled)
    {
        m_captureStream.reset(new ofstream(m_capturePath.c_str()));
    }
}

}   // SimInterface namespace
}   // RafMat namespace

